{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Dynamics: simulation and control\n",
    "This notebook focuses on the robot dynamics, for both simulating the behavior of the robot when forces are applied to it, and computing dynamic-aware control laws like computed torque.\n",
    "\n",
    "本笔记本主要关注机器人动力学，既用于模拟在对机器人施加力时其行为，也用于计算诸如计算力矩等动态感知控制律。\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 36,
   "metadata": {},
   "outputs": [],
   "source": [
    "import magic_donotload"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Set up \n",
    "We will use Pinocchio, the viewer and the QP solver quadprog. We use an ad-hoc model of a robot hand with simple geometry to make the contact simulation problem simple. \n",
    "\n",
    "我们将使用皮诺曹（Pinocchio）、查看器以及二次规划求解器quadprog。我们采用一个具有简单几何形状的机器人手的特定模型，以使接触模拟问题简化。 \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "metadata": {},
   "outputs": [],
   "source": [
    "import math\n",
    "import time\n",
    "import pinocchio as pin\n",
    "import numpy as np\n",
    "from numpy.random import rand\n",
    "from numpy.linalg import inv,pinv,norm,eig,svd\n",
    "import matplotlib.pylab as plt; plt.ion()\n",
    "import quadprog\n",
    "from tp4.robot_hand import RobotHand\n",
    "from utils.meshcat_viewer_wrapper import MeshcatVisualizer"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## QuadProg solver\n",
    "QuadProg is a simple QP solver that is very convenient to import in Python and very simple to use. \n",
    "\n",
    "A simple example using QuadProg is as follow. First randomly define a QP problem is\n",
    "\n",
    "$$\\min_x \\frac{1}{2} x^T A x - b^T x$$\n",
    "Under the constraints:\n",
    "$$C x \\ge d$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "metadata": {},
   "outputs": [],
   "source": [
    "A = np.random.rand(5,5)*2-1\n",
    "A = A @ A.T ### Make it positive symmetric 使其成为正对称的\n",
    "b = np.random.rand(5)\n",
    "\n",
    "C = np.random.rand(10, 5)\n",
    "d = np.random.rand(10)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The quadratic program can be solved by calling:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "metadata": {},
   "outputs": [],
   "source": [
    "[x,cost,_,niter,lag,iact] = quadprog.solve_qp(A,b,C.T,d)  # Notice that C.T is passed instead of C"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "where x is the solution, cost is the value of the cost function at x, niter is the number of iterations needed to obtain the optimum, lag is the dual optimum (lagrange multipliers) and iact are the active constraints (numbered from 1 to N, i.e. lag[iact-1] is not null). See the doc for more details.\n",
    "\n",
    "其中x是解，cost是x处成本函数的值，niter是获得最优解所需的迭代次数，lag是对偶最优解（拉格朗日乘子），iact是活动约束（从1到N编号，即lag[iact - 1]不为零）。更多详细信息请参阅文档。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "Help on cython_function_or_method in module quadprog:\n",
      "\n",
      "solve_qp(G, a, C=None, b=None, meq=0, factorized=False)\n",
      "    Solve a strictly convex quadratic program\n",
      "    \n",
      "    Minimize     1/2 x^T G x - a^T x\n",
      "    Subject to   C.T x >= b\n",
      "    \n",
      "    This routine uses the the Goldfarb/Idnani dual algorithm [1].\n",
      "    \n",
      "    References\n",
      "    ---------\n",
      "    ... [1] D. Goldfarb and A. Idnani (1983). A numerically stable dual\n",
      "        method for solving strictly convex quadratic programs.\n",
      "        Mathematical Programming, 27, 1-33.\n",
      "    \n",
      "    Parameters\n",
      "    ----------\n",
      "    G : array, shape=(n, n)\n",
      "        matrix appearing in the quadratic function to be minimized\n",
      "    a : array, shape=(n,)\n",
      "        vector appearing in the quadratic function to be minimized\n",
      "    C : array, shape=(n, m)\n",
      "        matrix defining the constraints under which we want to minimize the\n",
      "        quadratic function\n",
      "    b : array, shape=(m), default=None\n",
      "        vector defining the constraints\n",
      "    meq : int, default=0\n",
      "        the first meq constraints are treated as equality constraints,\n",
      "        all further as inequality constraints (defaults to 0).\n",
      "    factorized : bool, default=False\n",
      "        If True, then we are passing :math:`R^{−1}` instead of the matrix G\n",
      "        in the argument G, where :math:`G = R^T R` and R is upper triangular.\n",
      "    \n",
      "    Returns\n",
      "    -------\n",
      "    x : array, shape=(n,)\n",
      "        vector containing the solution of the quadratic programming problem.\n",
      "    f : float\n",
      "        the value of the quadratic function at the solution.\n",
      "    xu : array, shape=(n,)\n",
      "        vector containing the unconstrained minimizer of the quadratic function\n",
      "    iterations : tuple\n",
      "        2-tuple. the first component contains the number of iterations the\n",
      "        algorithm needed, the second indicates how often constraints became\n",
      "        inactive after becoming active first.\n",
      "    lagrangian : array, shape=(m,)\n",
      "        vector with the Lagragian at the solution.\n",
      "    iact : array\n",
      "        vector with the indices of the active constraints at the solution.\n",
      "\n"
     ]
    }
   ],
   "source": [
    "help(quadprog.solve_qp)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "More modern QP solver are able to handle simultaneously linear equality constraint and posses various strategy to fasten convergence (proximal merit function, Ruiz equilibration...  There is a very wide variety of optimization algorithm for QP problem, the reference book on those subject is:\n",
    "*nocedal j and s j wright. 2006. numerical optimization. springer new york*\n",
    "\n",
    "A recent solver made all the choices to bring the solver as close as possible to robotic type of problem solving : [proxQP](https://hal.inria.fr/hal-03683733/file/Yet_another_QP_solver_for_robotics_and_beyond.pdf)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## A robot hand for our test\n",
    "\n",
    "We are going to use a 4-finger hand, whose model is defined in Python (no urdf model) using capsule volumes.\n",
    "The hand is defined in the python code robot_hand.py. You can load and \n",
    "display it like this (don't forget to turn gepetto-viewer on with starting gepetto-gui).\n",
    "\n",
    "我们将使用一只四指手，其模型在Python中使用胶囊体体积进行定义（无urdf模型）。\n",
    "这只手在Python代码robot_hand.py中定义。你可以像这样加载并显示它（别忘了通过启动gepetto-gui打开gepetto-viewer）。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 41,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "You can open the visualizer by visiting the following URL:\n",
      "http://127.0.0.1:7005/static/\n"
     ]
    }
   ],
   "source": [
    "robot = RobotHand()\n",
    "viz = MeshcatVisualizer(robot)\n",
    "viz.display(robot.q0)\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {
    "scrolled": true
   },
   "outputs": [
    {
     "data": {
      "text/html": [
       "\n",
       "            <div style=\"height: 400px; width: 100%; overflow-x: auto; overflow-y: hidden; resize: both\">\n",
       "            <iframe src=\"http://127.0.0.1:7001/static/\" style=\"width: 100%; height: 100%; border: none\"></iframe>\n",
       "            </div>\n",
       "            "
      ],
      "text/plain": [
       "<IPython.core.display.HTML object>"
      ]
     },
     "execution_count": 7,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "viz.viewer.jupyter_cell()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Interactive sliders (for geeks)\n",
    "\n",
    "For this one, you need to additionally install [ipywidgets](https://github.com/jupyter-widgets/ipywidgets/blob/8.0.0b1/docs/source/examples/Widget%20Basics.ipynb). Execute the following then restart your jupyter process:\n",
    "\n",
    "```\n",
    "jupyter nbextension enable --py widgetsnbextension\n",
    "```\n",
    "\n",
    "\n",
    "Never mind, we don't really need it. You can also play with the hand by changing its configuration manually. \n",
    "\n",
    "Take care that the hand is small: zoom in to see it in the window.\n",
    "\n",
    "没关系，我们其实并不需要它。你也可以通过手动更改手部配置来进行操作。\n",
    "请注意手部模型较小：请放大窗口以便查看。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "564d2752d6ea42dd8b639e44a438e1f2",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(FloatSlider(value=0.7, description='thumb', max=1.5, min=-0.5, step=0.01), Output()), _d…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "518d7d2e58244172ba3b1779ee683f59",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(FloatSlider(value=0.3, description='others', max=2.0), Output()), _dom_classes=('widget-…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "try:\n",
    "    q0 = robot.q0.copy()\n",
    "    from ipywidgets import interact\n",
    "    @interact(thumb=(-.5, 1.5, 0.01))\n",
    "    def move_thumb(thumb=0.7):\n",
    "        q0[11:] = thumb\n",
    "        viz.display(q0)\n",
    "    @interact(others=(0., 2., 0.1))\n",
    "    def move_fingers(others=0.3):\n",
    "        q0[1:11] = others\n",
    "        viz.display(q0)\n",
    "except:\n",
    "    print('iPyWidget should be installed, see above. ')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Remember that the model rendered in the viewer is just a display, not a simulation (yet).  You can pretty much achieve any movements you want in this display, because we are not enforcing (yet) any physical law. For example, if you want to move the fingers following any trajectories you like, you can do:\n",
    "\n",
    "请记住，在查看器中渲染的模型只是一种展示，（目前）并非模拟。在这种展示中，你几乎可以实现任何你想要的动作，因为（目前）我们并未强制实施任何物理定律。例如，如果你想让手指按照任何你喜欢的轨迹移动，你可以这样做："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = robot.q0.copy()\n",
    "\n",
    "for i in range(500): # Put 1000 or 5000 if you want a longer move.\n",
    "    for iq in range(3,robot.model.nq):\n",
    "        q[iq] = -1+np.cos(i*1e-2*(1+iq/5))\n",
    "    viz.display(q)\n",
    "    time.sleep(2e-3)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Forward unconstrained dynamics\n",
    "### Computing the acceleration"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In a first time, let's play with the dynamics without constraints (hence without quadprog).\n",
    "\n",
    "Choosing an arbitrary joint torque $\\tau_q$, pinocchio can compute the robot acceleration and integrate it.\n",
    "\n",
    "The dynamic equation of the robot is $M a_q + b = \\tau_q$, with $M$ the mass, $a_q$ the joint acceleration and $b$ the drift.\n",
    "The mass matrix can be computed using *CRB* algorithm (function of q). The drift is computed using *NLE* (nonlinear effects) algorithm (function of $q$, $v_q$). Which contains normal gravity and coriolis effect.\n",
    "\n",
    "首先，让我们在无约束（因此不使用二次规划）的情况下研究动力学。\n",
    "\n",
    "选择任意的关节扭矩 $\\tau_q$，Pinocchio 可以计算机器人的加速度并进行积分。\n",
    "\n",
    "机器人的动力学方程为 $M a_q + b = \\tau_q$，其中 $M$ 是质量矩阵，$a_q$ 是关节加速度，$b$ 是漂移项。\n",
    "质量矩阵可以使用 *CRB* 算法（$q$ 的函数）计算。漂移项使用 *NLE*（非线性效应）算法（$q$、$v_q$ 的函数）计算。其中包含正常重力和科里奥利效应。 "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = robot.q0.copy()\n",
    "vq = np.zeros(robot.model.nv)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 15,
   "metadata": {},
   "outputs": [],
   "source": [
    "M = pin.crba(robot.model, robot.data, q)\n",
    "b = pin.nle(robot.model, robot.data, q, vq)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "These terms correspond to the inverse dynamics. They can be numerically inverted to compute the direct dynamics.\n",
    "\n",
    "这些项对应于逆动力学。可以通过数值求逆来计算正向动力学。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Using $M$ and $b$ computed by the above algorithms, and knowing a given set of joint torques $\\tau_q$, how would you compute the accelration $a_q$ `aq` induce by the torques ?\n",
    "\n",
    "使用上述算法计算出的$M$和$b$，并且已知一组给定的关节扭矩$\\tau_q$，那么如何计算由这些扭矩引起的加速度$a_q$（即“aq”）呢？"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [],
   "source": [
    "tauq = np.random.rand(robot.model.nv)\n",
    "aq = np.random.rand(robot.model.nv)  # Replace me"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is the solution"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 17,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %do_not_load tp4/generated/solution_pd_dyninv\n",
    "\n",
    "tauq = np.random.rand(robot.model.nv)\n",
    "aq = inv(M) @ (tauq - b)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The inverse-dynamics algorithm indeed compute the needed torques to achieve a given acceleration. We can use the *RNEA* (for forward dynamics) function to double-check our result:\n",
    "\n",
    "逆动力学算法确实可以计算出实现给定加速度所需的扭矩。我们可以使用*RNEA*（用于正向动力学）函数来复核我们的结果："
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "3.053144857983103e-15\n"
     ]
    }
   ],
   "source": [
    "print(norm(pin.rnea(robot.model,robot.data,q,vq,aq)-tauq))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Integrating the acceleration\n",
    "\n",
    "Once aq as been computed, it is straight forward to integrate it to velocity using $v_q = v_q + a_q \\Delta t$. \n",
    "Integration to joint position is more complex in general, as we saw for the mobile robot. Here, $n_q = n_v$ and everything is Euclinea, so a simple += integration would work, but since it is implemented in pinocchio, let's keep the good habits and use general integration.\n",
    "\n",
    "一旦计算出\\(a_q\\)，使用\\(v_q = v_q + a_q \\Delta t\\)将其积分得到速度就很直接。\n",
    "如我们在移动机器人案例中所见，一般来说，积分得到关节位置更为复杂。这里，\\(n_q = n_v\\)且所有内容都是欧几里得的，所以简单的\\(+=\\)积分可行，但由于它是在Pinocchio中实现的，让我们保持良好习惯并使用通用积分。 "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "dt = 2e-3\n",
    "N_steps = 5000"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "vq += np.random.rand(robot.model.nv)  # Replace me\n",
    "q = np.random.rand(robot.model.nq)  # Replace me"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And a solution.."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/solution_pd_integrate"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now, you should be able to implement a first simulation (not a rendering any more) of the robot behavior when no torque is applied (tauq = 0).\n",
    "\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Init simulation"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = robot.q0.copy()\n",
    "viz.display(q)\n",
    "vq = np.zeros(robot.model.nv)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Fill the template below to get a free-falling dynamics."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "for it in range(N_steps):\n",
    "    t = it*dt\n",
    "\n",
    "    # Retrieve the dynamics quantity at time t\n",
    "    M = np.eye(robot.model.nv)        # REPLACE ME\n",
    "    b = np.zeros(robot.model.nv)      # REPLACE ME\n",
    "\n",
    "    # Compute the force that apply\n",
    "    tauq = np.zeros(robot.model.nv)\n",
    "\n",
    "    # Use generalized PFD to calculate aq\n",
    "    aq = np.zeros(robot.model.nv)     # REPLACE ME\n",
    "\n",
    "    # Double integration to update vs and q\n",
    "    vq = np.zeros(robot.model.nv)    # REPLACE ME\n",
    "    q  = np.zeros(robot.model.nq)   # REPLACE ME\n",
    "\n",
    "    # Visualization\n",
    "    if it%20==0: \n",
    "        viz.display(q)\n",
    "        time.sleep(20*dt)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is the solution"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "# %do_not_load tp4/generated/solution_pd_freefall\n",
    "\n",
    "for it in range(N_steps):\n",
    "    t = it*dt\n",
    "\n",
    "    # Dynamics at time t\n",
    "    M = pin.crba(robot.model, robot.data, q)\n",
    "    b = pin.nle(robot.model, robot.data, q, vq)\n",
    "\n",
    "    # Force that apply\n",
    "    tauq = np.zeros(robot.model.nv)\n",
    "\n",
    "    # PFD\n",
    "    aq = inv(M) @ (tauq - b)\n",
    "\n",
    "    # Double integration\n",
    "    vq += aq * dt\n",
    "    q = pin.integrate(robot.model, q, vq * dt)\n",
    "\n",
    "    # Visualization\n",
    "    if it%20==0: \n",
    "        viz.display(q)\n",
    "        time.sleep(20*dt)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now modify the simulator code above to encompass joint friction, i.e. the torque is opposite to the velocity with a friction coefficient $K_f$ (take $K_f=0.1$ as a start)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = robot.q0.copy()\n",
    "viz.display(q)\n",
    "vq = np.zeros(robot.model.nv)\n",
    "Kf = 0.1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Use the same loop templace as above"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And here the solution"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/solution_pd_frictionfall"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Proportional-derivative and computed torque"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Now choose a reference joint position.\n",
    "The joint torques can then be computed to track the desired position, with $$\\tau_q = -K_p (q-q^{des}) - K_v (\\dot{q} - \\dot{q}^{des})$$Both gains $K_p$ and $K_v$ should be properly chosen. Optimal tracking is obtained with $K_v = 2 \\sqrt{K_p}$. \n",
    "In general, we can also add a force proportional to the desired acceleration. Indeed, when tracking is done correctly the PD controll gives a zero torque. To drive the acceleration toward the good sign we can add an acceleration contribution:\n",
    "\n",
    "$$\\tau_q = -K_p (q-q^{des}) - K_v (\\dot{q} - \\dot{q}^{des}) + \\alpha \\ddot{q}^{des}$$\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "First choose the PD gains and simulation parameter:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = np.zeros(robot.model.nq)\n",
    "viz.display(q)\n",
    "vq = np.zeros(robot.model.nv)\n",
    "# Hyperparameters for the control and the simu\n",
    "Kf = 0.1\n",
    "Kp = 50.               # proportional gain (P of PD)\n",
    "Kv = 2 * np.sqrt(Kp)   # derivative gain (D of PD)\n",
    "alpha = 1.\n",
    "dt = 2e-3             # simulation timestep\n",
    "N_steps = 5000"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Then choose the reference trajectory."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [],
   "source": [
    "from tp4.traj_ref import TrajRef\n",
    "qdes = TrajRef(robot.q0,omega = np.array([0.,.1,1,1.5,2.5,-1,-1.5,-2.5,.1,.2,.3,.4,.5,.6]),amplitude=1.5)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Finally, implement the control loop using the following template."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [],
   "source": [
    "for it in range(N_steps):\n",
    "    t = it*dt\n",
    "\n",
    "    # Retrieve the dynamics quantity at time t\n",
    "    M = np.eye(robot.model.nv)        # REPLACE ME\n",
    "    b = np.zeros(robot.model.nv)      # REPLACE ME\n",
    "\n",
    "    # Compute the force that apply\n",
    "    tauq = np.zeros(robot.model.nv)\n",
    "\n",
    "    # Use generalized PFD to calculate aq\n",
    "    aq = np.zeros(robot.model.nv)     # REPLACE ME\n",
    "\n",
    "    # Double integration to update vs and q\n",
    "    vq = np.zeros(robot.model.nv)    # REPLACE ME\n",
    "    q  = np.zeros(robot.model.nq)   # REPLACE ME\n",
    "\n",
    "    # Visualization\n",
    "    if it%20==0: \n",
    "        viz.display(q)\n",
    "        time.sleep(20*dt)\n"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here is the solution, should you need it."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/solution_pd_loop"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Here, there is a strong coupling between joints, due to the mass matrix that is not compensated in the simple PD law. In theory, the computed torques is to compute the joint torque by inverse dynamics from a reference joint acceleration. This boils down to canceling the simulation equation by choosing the proper terms in the control law. It is now very interesting to implement in case of perfect dynamics knowledge. It might be more interesting to study in case the simulation is done with the perfect M, while the control is computed with approximate M (for example, using only the diagonal terms of the mass matrix). Let's rather simulate contact.\n",
    "\n",
    "在这里，由于简单的PD控制律中未对质量矩阵进行补偿，关节之间存在很强的耦合。理论上，计算力矩法是通过参考关节加速度，利用逆动力学来计算关节扭矩。这归根结底是通过在控制律中选择合适的项来抵消模拟方程。在对动力学有完美认知的情况下，实现这一点会非常有趣。如果在模拟时使用精确的质量矩阵M，而在计算控制量时使用近似的M（例如，仅使用质量矩阵的对角项），对这种情况进行研究可能会更有意思。我们不妨模拟一下接触情况。 "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is intersting to plot the results. In the solution of the previous example, the effective and desired position q and qdes have been stored in a log list. We can display them with matplotlib.\n",
    "绘制结果很有趣。在上一个例子的解决方案中，有效位置和期望位置q和qdes已存储在一个日志列表中。我们可以用matplotlib显示它们。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [],
   "source": [
    "def plot_joint_profiles(i, hq, hqdes):\n",
    "    plt.subplot(111)\n",
    "    plt.plot([q[i] for q in hq])\n",
    "    plt.plot([q[i] for q in hqdes])\n",
    "    plt.ylabel(f'Joint {i}')\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'hq' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[31], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m plot_joint_profiles(\u001b[38;5;241m11\u001b[39m, \u001b[43mhq\u001b[49m, hqdes)\n",
      "\u001b[0;31mNameError\u001b[0m: name 'hq' is not defined"
     ]
    }
   ],
   "source": [
    "plot_joint_profiles(11, hq, hqdes)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Collision checking"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Pinocchio is built on top of HPP FCL (a fork of the Free-Collision-Lib) to compute collision and distances. For this notebook, a wrapper as been set up to make the computation easier.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [],
   "source": [
    "from tp4.collision_wrapper import CollisionWrapper\n",
    "colwrap = CollisionWrapper(robot)\n",
    "colwrap.computeCollisions(q)\n",
    "collisions = colwrap.getCollisionList()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "`getCollisionList` returns a list of all the active collisions. The list containts triplets `i,c,r`, with `i` the index of the collision pair, `c` the collition pair and `r` a structure containg the collision details, in particular the collision position in world frame and the collision normal. \n",
    "\n",
    "`getCollisionList` 返回所有活动碰撞的列表。该列表包含三元组 `i,c,r`，其中 `i` 是碰撞对的索引，`c` 是碰撞对，`r` 是一个包含碰撞详细信息的结构，特别是世界坐标系中的碰撞位置和碰撞法线。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Collision detection is done using the state of the art GJK collision detection algorithm.\n",
    "Without saying its name, it is a Frank Wolf optimization scheme on the minkowski difference using the so called \"support function\".\n",
    "\n",
    "碰撞检测采用了最先进的GJK碰撞检测算法。\n",
    "无需提及它的名字，它是一种基于闵可夫斯基差的Frank Wolf优化方案，使用了所谓的 “支撑函数”。\n",
    "\n",
    "A nice blog post on GJK: https://cse442-17f.github.io/Gilbert-Johnson-Keerthi-Distance-Algorithm/"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "GJK can easilly be extended with EPA to find the distance and normal to collision. Then because shapes are configuration parametrized, we can deduce a kinematic jacobian that translate configuration speed into normal speed of the collision.\n",
    "\n",
    "GJK算法可以很容易地与EPA算法结合进行扩展，以找到碰撞距离和碰撞法线。由于形状是由配置参数化的，因此我们可以推导出运动学雅可比矩阵，将配置速度转换为碰撞的法向速度。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "From this list, it is possible to compute various quantities, in particular the contact Jacobian (corresponding to the normal direct of each contact), which convert configuration velocities into veolocity of the contact interfaces. The contact distances noted $e$, and the self-acceleration of the contact point (similar to Coriolis acceleration, i.e. due to the velocity of the joints, also denoted by $\\dot{J}\\dot{q}$.\n",
    "\n",
    "从这个列表中，可以计算各种量，特别是接触雅可比矩阵（对应于每个接触的法向方向），它将构型速度转换为接触界面的速度。接触距离记为$e$，接触点的自加速度（类似于科里奥利加速度，即由于关节速度引起的加速度，也记为$\\dot{J}\\dot{q}$）。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [],
   "source": [
    "dist=colwrap.getCollisionDistances(collisions)\n",
    "J = colwrap.getCollisionJacobian(collisions)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "The contact accelereation is indeed $$v_c = \\frac{d e}{dt} = J \\dot{q}$$ $$a_c = \\frac{d v_c}{dt} = \\frac{d J \\dot{q}}{dt} = J \\ddot{q} + \\dot{J}\\dot{q}$$where $\\dot{J}\\dot{q}$ is a Coriolis term. This can be computed if calling `colwrap.computeCollision(q,vq)` (add the vq argument). Now, `colwrap.getCollisionJdotQdot` will return the self-acceleration."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [],
   "source": [
    "colwrap.computeCollisions(q, vq)\n",
    "collisions = colwrap.getCollisionList()\n",
    "dist = colwrap.getCollisionDistances(collisions)\n",
    "J = colwrap.getCollisionJacobian(collisions)\n",
    "JdotQdot = colwrap.getCollisionJdotQdot()"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Better try this with a configuration of the hand where there are some collisions. Here is everything needed to solve a contact problem."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Contact simulator"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 1. Enforce positive acceleration \n",
    "\n",
    "The joint acceleration is constrained by the contact constraint: it can only go appart. It can be written as a minimization problem using Gauss principle:\n",
    "$$min \\quad \\frac{1}{2}  (\\ddot{q} - \\ddot{q}_0 )^\\top M (\\ddot{q} - \\ddot{q}_0 )$$\n",
    "$$s.t. \\quad a_c \\geq 0 $$\n",
    "where $ \\ddot{q}_0 = M^{-1} (\\tau-b)$ is the free acceleration, i.e. the acceleration obtained in Question 2 where no constraint is active.\n",
    "\n",
    "This model is physical, but because the simulation is time stepped, we need to adapt the physic to get a simulation closer to the reality.\n",
    "\n",
    "关节加速度受接触约束的限制：它只能分离。根据高斯原理，它可以写成一个最小化问题：\n",
    "$$\\min \\quad \\frac{1}{2}  (\\ddot{q} - \\ddot{q}_0 )^\\top M (\\ddot{q} - \\ddot{q}_0 )$$\n",
    "$$\\text{s.t.} \\quad a_c \\geq 0 $$\n",
    "其中$\\ddot{q}_0 = M^{-1} (\\tau - b)$是自由加速度，即在问题2中无约束作用时得到的加速度。\n",
    "\n",
    "这个模型是基于物理原理的，但由于模拟是按时间步长进行的，我们需要调整物理模型，以使模拟更接近现实。\n",
    "\n",
    "### 2. Impact\n",
    "When creating the contact with a non-zero velocity, enforcing a null acceleration is not sufficient as the two bodies will penetrate during the time step. When a new contact is created, the velocity of the contact normals should be nullified to render physic. You can do that by *projecting the current velocity in the null space of the normal jacobian of the new contacts*. Ideally, this projection should be done while respecting the Gauss principle, but that is a little bit too advanced for the purpose of this notebook.\n",
    "\n",
    "在创建具有非零速度的接触时，仅强制加速度为零是不够的，因为在时间步长内两个物体会发生穿透。当创建新的接触时，接触法线的速度应该归零以实现物理模拟。你可以通过*将当前速度投影到新接触的法向雅可比矩阵的零空间中*来实现这一点。理想情况下，这种投影应该在遵循高斯原理的同时进行，但对于本笔记本的目的来说，这有点过于高级了。\n",
    "\n",
    "### 3. Penetration correction\n",
    "Enforcing 0 velocity at impact and 0 acceleration later should be enough in theory. Yet some numerical drift might push the bodies inside each other. We can then enforce the contact acceleration to be at least a PD pushing the two volums out of each other to nullify the numerical drift effect. We enforce $a_c >= PD(e, v_v)$. The constraint become:\n",
    "$$ J \\ddot{q} + \\dot{J}\\dot{q} \\ge -K_p^c e -K_v^c J \\dot{q}$$\n",
    "\n",
    "\n",
    "理论上，在碰撞时强制速度为0，之后强制加速度为0应该就足够了。然而，一些数值漂移可能会使物体相互重叠。然后，我们可以强制接触加速度至少为一个将两个物体推开的PD（比例 - 微分控制）值，以消除数值漂移的影响。我们强制 $a_c \\geq PD(e, v_v)$。约束条件变为：\n",
    "$$ J \\ddot{q} + \\dot{J}\\dot{q} \\geq -K_p^c e -K_v^c J \\dot{q}$$\n",
    "\n",
    "### 4. Final numerical problem\n",
    "The problem can be rewritten as a QP:\n",
    "$$min \\quad \\frac{1}{2} \\ddot{q}^T M \\ddot{q} - (M \\ddot{q}_0)^\\top \\ddot{q} + (\\frac{1}{2} \\ddot{q}_0^T M \\ddot{q}_0)    $$\n",
    "$$s.t. \\quad J \\ddot{q} \\ge -K_p^c e - K_v^c J \\dot{q} - \\dot{J}\\dot{q} $$"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "You are now all set up for implementing the contact simulator.\n",
    "The complete loop should be as follows:\n",
    "- torque is computed from a PD tracking a time-varying joint position (question 3).\n",
    "- After computing torque, all collision pairs must be checked to find those with distances collision penetrating more than $10 ^{-4}$\n",
    "- Corresponding Jacobians must be computed and stacked. If a new collision has appeared, the joint velocity must be projected to nullify it.\n",
    "- If not collision is active, the joint acceleration is computed from inverting the mass matrix (question 2).\n",
    "- Otherwise, it is computed using QP (question 4). With the PD corrected contact constraint.\n",
    "- The resulting acceleration is integrated twice (question 1) before displaying the robot starting a new simulation iteration.\n",
    "\n",
    "You can start from the same template as the freecontact solver.\n",
    "\n",
    "现在你已准备好实现接触模拟器。\n",
    "完整的循环应如下所示：\n",
    "- 根据追踪时变关节位置的PD算法计算扭矩（问题3）。\n",
    "- 计算扭矩后，必须检查所有碰撞对，找出距离穿透超过$10 ^{-4}$的碰撞对。\n",
    "- 必须计算并堆叠相应的雅可比矩阵。如果出现新的碰撞，必须对关节速度进行投影以消除它。\n",
    "- 如果没有活动的碰撞，则通过反转质量矩阵计算关节加速度（问题2）。\n",
    "- 否则，使用QP算法计算（问题4），并带有PD校正的接触约束。\n",
    "- 在显示机器人开始新的模拟迭代之前，将得到的加速度积分两次（问题1）。\n",
    "你可以从与自由接触求解器相同的模板开始。 "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [],
   "source": [
    "q = np.zeros(robot.model.nq)\n",
    "viz.display(q)\n",
    "vq = np.zeros(robot.model.nv)\n",
    "\n",
    "# Hyperparameters for the control and the simu\n",
    "Kf = 0.1\n",
    "Kp = 50.               # proportional gain (P of PD)\n",
    "Kv = 2 * np.sqrt(Kp)   # derivative gain (D of PD)\n",
    "alpha = 1.\n",
    "\n",
    "dt = 1e-3             # simulation timestep\n",
    "N_steps = 10000\n",
    "\n",
    "Kp_c = 100.\n",
    "Kv_c = 2 * np.sqrt(Kp_c)\n",
    "\n",
    "qdes = TrajRef(robot.q0,omega = np.array([0.,.1,1,1.5,2.5,-1,-1.5,-2.5,.1,.2,.3,.4,.5,.6]),amplitude=1.5)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Start from the previous templates and add the contact simulation."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "And the solution"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%do_not_load tp4/generated/solution_contact_simulator"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 35,
   "metadata": {},
   "outputs": [
    {
     "ename": "NameError",
     "evalue": "name 'hq_c' is not defined",
     "output_type": "error",
     "traceback": [
      "\u001b[0;31m---------------------------------------------------------------------------\u001b[0m",
      "\u001b[0;31mNameError\u001b[0m                                 Traceback (most recent call last)",
      "Cell \u001b[0;32mIn[35], line 1\u001b[0m\n\u001b[0;32m----> 1\u001b[0m plot_joint_profiles(\u001b[38;5;241m11\u001b[39m, \u001b[43mhq_c\u001b[49m, hqdes_c)\n",
      "\u001b[0;31mNameError\u001b[0m: name 'hq_c' is not defined"
     ]
    }
   ],
   "source": [
    "plot_joint_profiles(11, hq_c, hqdes_c)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "What are all the problematic that can be explored ?\n",
    "- integrators\n",
    "- QP solvers\n",
    "- Other physic principles (e.g. Coulomb)\n",
    "- Other controller than PD\n",
    "- Numerical conditioning, footprint, convergence of algorithms"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Extension"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Instead of computing the torque $\\tau_q$ as the PD of a trajectory following in the configuration space, we can have different trajectory task in different spaces, then we can compute a PD for each task while being constraint to the kernel space of the previous tasks. We can use the result of the hierarchical procedure as a torq control instead."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We can have a hierarchy that change during time e.g. for manipulation planning."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "robotics_course",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.10.17"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
